#include "rocker.h"
//#include "NVIC.h"


// 配置两个摇杆电位器的IO
void GPIO_Rocker_Config(){
	/*
	右摇杆X:R_X - P0.1 - ADC9
	右摇杆Y:R_Y - P0.0 - ADC8
	左摇杆X:L_X - P1.1 - ADC1
	左摇杆Y:L_Y - P1.0 - ADC0
	*/
	GPIO_InitTypeDef init;
	
	// 右摇杆
	init.Pin = GPIO_Pin_0 | GPIO_Pin_1;
	init.Mode = GPIO_HighZ; // 这里需要配置为高阻模式
	GPIO_Inilize(GPIO_P0, &init);
	
	// 左摇杆
	init.Pin = GPIO_Pin_0 | GPIO_Pin_1;
	init.Mode = GPIO_HighZ; // 这里需要配置为高阻模式
	GPIO_Inilize(GPIO_P1, &init);
}


// ADC配置 -- 对于模数转换器没有引脚的开关、切换，配置了IO去取值即可
void ADC_Config(){
	ADC_InitTypeDef init;
	// 模数转换通道
	init.ADC_SMPduty = 31;		//ADC 模拟信号采样时间控制, 0~31（注意： SMPDUTY 一定不能设置小于 10）
	init.ADC_Speed = ADC_SPEED_2X1T;			//设置 ADC 工作时钟频率	ADC_SPEED_2X1T~ADC_SPEED_2X16T
	init.ADC_AdjResult = ADC_RIGHT_JUSTIFIED;	//ADC结果调整,	ADC_LEFT_JUSTIFIED(左对齐),ADC_RIGHT_JUSTIFIED(右对齐)
	init.ADC_CsSetup = 0;		//ADC 通道选择时间控制 0(默认),1
	init.ADC_CsHold = 1;		//ADC 通道选择保持时间控制 0,1(默认),2,3
	
	ADC_Inilize(&init);
	
	// 启动 adc / adc电源开关
	ADC_PowerControl(ENABLE);
	
	// adc中断使能
	// NVIC_ADC_Init(DISABLE, Priority_0);
}


// 摇杆电位器初始化
void Rocker_init(){
	GPIO_Rocker_Config();
	ADC_Config();
}


// 右摇杆电位器测量值转为摇杆角度
void get_rocker_angle_right(int *angle){
	/*
	  左摇杆X:L_X - P1.1 - ADC1
	  左摇杆Y:L_Y - P1.0 - ADC0
	 */
	int res_x, res_y, sta_min_x, sta_max_x, sta_min_y, sta_max_y, x, y;
	
	// 获取右摇杆电位器的值
	res_x = Get_ADCResult(ADC_CH9);
	res_y = Get_ADCResult(ADC_CH8);
	
	sta_min_x = ROCKER_R_STA_X - ROCKER_R_STA_RANGE;
	sta_max_x = ROCKER_R_STA_X + ROCKER_R_STA_RANGE;
	sta_min_y = ROCKER_R_STA_Y - ROCKER_R_STA_RANGE;
	sta_max_y = ROCKER_R_STA_Y + ROCKER_R_STA_RANGE;
	
	// 在静止允许波动范围内认为角度为0
	if((res_x > sta_min_x && res_x < sta_max_x) && (res_y > sta_min_y && res_y < sta_max_y)){
		*angle = 0;
	}
	
	// 依据x，y的值来计算角度---与Y轴正方向的夹角(y轴右边为正，左边为负)
	else {
		// 建立以上为正，以右为正的坐标系
		x = res_x - ROCKER_R_STA_X;
		y = -(res_y - ROCKER_R_STA_Y);
		
		if((x >= 0 && y >= 0) || (x < 0 && y >= 0)){
			// 第一第二象限内角度就是arctan(x/y)
			// 取tan(x/y)的反三角函数值，得到弧度，再换算为角度
			// 两个整数/会取整运算，所以需要转换我小数除
			*angle = (int)(atan((double)x / (double)y) * 180 / 3.14);
		}
		
		else if(x < 0 && y < 0){
			// 第三象限内角度为-arctan(y/x) - 90
			*angle = -(int)(atan((double)y / (double)x) * 180 / 3.14) - 90;
		}
		
		else {
			// 第四象限内角度为-arctan(y/x) + 90
			*angle = -(int)(atan((double)y / (double)x) * 180 / 3.14) + 90;
		}
	}
	
//	printf("%d\n", *angle);
}


// 将角度转换为给小车的信号
void angle_2_car_signal(u8 *signal){
	int angle;
	// 获取角度
	get_rocker_angle_right(&angle);
	
	// 判断角度，选择信号
	if(angle == 0){
		// 角度为0，停止
		*signal = CAR_STOP;
	}
	else if(angle >= -CAR_WARD_ANGLE_RANGE && angle <= CAR_WARD_ANGLE_RANGE){
		// 判定为前进
		*signal = CAR_FORE;
	}
	else if(angle >= (180 - CAR_WARD_ANGLE_RANGE) || angle <= -180 + CAR_WARD_ANGLE_RANGE){
		// 判定为后退
		*signal = CAR_BACK;
	}
	else if(angle > 0){
		// 判定为右转
		*signal = CAR_RIGHT;
	}
	else {
		// angle < 0 判定为左转
		*signal = CAR_LEFT;
	}
}


// 左摇杆电位器测量值转为摇杆角度
void get_rocker_angle_left(u16 *angle){
	/*
	  右摇杆X:R_X - P0.1 - ADC9
	  右摇杆Y:R_Y - P0.0 - ADC8
	 */
	int res_x, res_y;
	res_x = Get_ADCResult(ADC_CH1);
	res_y = Get_ADCResult(ADC_CH0);
	printf("x = %d\n", res_x);
	printf("y = %d\n", res_y);
}

